#include "TransformationHomogene.hpp"

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using namespace boost::numeric::ublas;

TransformationHomogene::TransformationHomogene( const MatriceSO3& rotation )
    : matrix< double >( zero_matrix< double >( 4u, 4u ) )
{
    matrix_range< matrix< double > > rotPart( *this, range( 0u, 3u ),
                                              range( 0u, 3u ) );
    rotPart = rotation;

    operator()( 3u, 3u ) = 1.;
}

TransformationHomogene::TransformationHomogene( const Vecteur& translation )
    : matrix< double >( identity_matrix< double >( 4u, 4u ) )
{
    matrix_column< matrix< double > > lastColumn( *this, 3u );

    lastColumn( 0u ) = translation.x();
    lastColumn( 1u ) = translation.y();
    lastColumn( 2u ) = translation.z();
}

TransformationHomogene::TransformationHomogene( const double& homothetie )
    : matrix< double >( identity_matrix< double >( 4u, 4u ) )
{
    for ( size_t i( 0u ); i < 3u; ++i )
        operator()( i, i ) = homothetie;
    operator()( 3u, 3u ) = 1.;
}

TransformationHomogene::TransformationHomogene( const TransformationHomogene& autre )
    : MatriceBase( autre )
{}

TransformationHomogene::~TransformationHomogene()
{}

TransformationHomogene& TransformationHomogene::operator=( const TransformationHomogene& autre )
{
    MatriceBase::operator=( autre );

    return *this;
}

TransformationHomogene& TransformationHomogene::operator*=( const TransformationHomogene& autre )
{
    *this = prod( *this, autre );

    return *this;
}

TransformationHomogene TransformationHomogene::operator*( const TransformationHomogene& autre )
{
    TransformationHomogene tmp( *this );

    return tmp *= autre;
}

const double& TransformationHomogene::at( const unsigned& i,
                                          const unsigned& j ) const
{
    return operator()( i, j );
}

bool TransformationHomogene::inverse( TransformationHomogene& inv ) const
{
    MatriceBase A( *this );

    permutation_matrix< double > pm( A.size1() );

    int res = lu_factorize( A, pm );
    if ( res != 0 )
        return false;

    inv.assign( identity_matrix< double > ( A.size1() ) );

    lu_substitute( A, pm, inv );

    return true;
}

TransformationHomogene& TransformationHomogene::operator =( const MatriceBase& mat )
{
    MatriceBase::operator=( mat );

    return *this;
}
